#include <src/user.h>
#include "user_queue.h"
#include "CarPIDCtrl.h"
#include "sfud.h"
#include "sfud_cfg.h"
#include "dfs_posix.h"
#define LOG_TAG     "user.c"     // 该模块对应的标签。不定义时，默认：NO_TAG
#define LOG_LVL     LOG_LVL_DBG   // 该模块对应的日志输出级别。不定义时，默认：调试级别

#include <ulog.h>

/******************************公共**************************************/
rt_event_t uart_recive_ctrl;//串口接收事件控制块,所有串口共用
/******************************线程**************************************/
rt_thread_t ldv_uart_recive_thread;//LDV数据解析线程
rt_thread_t pc_uart_recive_thread;//PC数据解析线程
rt_thread_t pc_uart_send_thread;//PC数据发送线程
rt_thread_t car_can_thread;//小车CAN通信线程
rt_thread_t flash_spi_thread;//W25Q64操作线程
/******************************LDV串口解析*******************************/
rt_device_t u5_dev = RT_NULL, u6_dev = RT_NULL, u7_dev = RT_NULL;
rt_uint32_t ldv_rcv_event;//LDV串口接收事件接收变量
rt_int16_t i16_val;//解析临时变量
rt_uint16_t u16_val;//解析临时变量
float f_val;//解析临时变量
double df_val;//解析临时变量
struct s_board_data sBoardData; //信号板一帧数据
/******************************PC串口收发********************************/
#define PC_UART "uart1"
rt_event_t pc_uart_event;
rt_device_t pc_uart = RT_NULL;
rt_device_t hw_timer0 = RT_NULL;
rt_uint32_t pc_rcv_event;//PC串口接收事件接收变量

rt_uint8_t ldv_to_pc_buf[37] = {0XA6,0X6A,0X25,0XD0};//LDV TO PC 发送缓存区   5+32
rt_uint8_t car_to_pc_buf[13] = {0XA6,0X6A,0X0D,0XA0};//CAR TO PC 发送缓存区   5+8
rt_uint8_t pid_to_pc_buf[17] = {0XA6,0X6A,0X11,0XC0};  //PID TO PC 5+12
rt_uint8_t irq_to_pc_buf[10] = {0XA6,0X6A,0X0A,0XB0};//EX_IRQ TO PC 发送缓存区  5+5


rt_mq_t pc_send_msg;//向PC发送的消息队列
rt_uint8_t msg_pool[3000];//消息队列内存池
rt_uint8_t msg_rcv_buf[MSG_MAX_LEN];//一条消息缓存区
/******************************小车_CAN*********************************/
rt_uint32_t pc_alive_cnt = 0;   // pc指令心跳计数
rt_timer_t car_timer;//定50ms循环发送CAN指令，定时50ms更新指令
rt_tick_t car_timer_outcnt = 50;//定时器超时时间 50ms
struct car_ctrl_parameter car_send_parameter;//下发的参数结构体
rt_uint8_t car_pid_en;//PID开关
float car_dir_err;//在PC接收中得到
rt_device_t car_can;
rt_sem_t car_rx_sem;//CAN接收数据信号量
struct rt_can_filter_item car_can_filter;//CAN 接收过滤表

struct rt_can_msg can_send_buf =
        {
               .id = 0x18C4D2D0,
               .ide = RT_CAN_EXTID,
               .rtr = RT_CAN_DTR,
               .len = 8,
               .data = {0,1,2,3,4,5,6,7}
        };//CAN 发送BUF
struct rt_can_msg can_rcv_buf; //CAN 接收BUF
/******************************FLASH SPI********************************/
rt_device_t spi01_dev = RT_NULL;
static struct rt_spi_device spi0_w25q64_dev;//SPI设备控制句柄
sfud_flash_t w25q64_dev = RT_NULL;                //FLASH设备控制句柄
int config_fd;//文件管理句柄
struct flash_save_data saved_param;//FLASH已经保存的参数缓存
struct flash_save_data saving_param;//等待保存进FLASH的参数缓存
/******************************外部触发*********************************/
#define EX_TRIGGER_PIN1  "P505"
rt_bool_t ex_trigger_flag = 0;//外部光电开关触发标志
rt_uint8_t calc_distance_flag;//计算里程标志
/******************************Main************************************/
void user_main()
{
    if (RT_EOK !=spi_flash_mount())
    {
        if (RT_EOK ==flash_mkfs())
        {
            LOG_I("W25Q64BV mkfs OK!");
            if(RT_EOK == spi_flash_mount())
            {
                LOG_I("W25Q64BV flash mount to '/W25Q64BV'");
            }
            else
            {
                LOG_I("W25Q64BV flash mount failed");
            }
        }
        else
        {
            LOG_W("W25Q64BV mkfs and mount failed!");
        }
    }
    else
    {
        LOG_I("W25Q64BV flash mount to '/W25Q64BV'");
    }
    uart_recive_ctrl = rt_event_create("uart_rcv", RT_IPC_FLAG_FIFO);//创建串口接收事件，在各个串口接收中断中发送事件
    pc_uart_event = rt_event_create("pc_uart_rcv", RT_IPC_FLAG_FIFO);
    //注意线程优先级
    flash_spi_thread = rt_thread_create("flash_spi_thread",flash_spi_entry,RT_NULL,2048,10,5);
    pc_uart_recive_thread = rt_thread_create("pc_recive_thread", pc_uart_entry, RT_NULL, 2048, 11, 5);
    pc_uart_send_thread = rt_thread_create("pc_send_thread", pc_uart_send_entry, RT_NULL, 2048, 11, 10);
    ldv_uart_recive_thread = rt_thread_create("ldv_rcv_thread", ldv_uart_recive_entry, RT_NULL, 5000, 12, 10);
    car_can_thread = rt_thread_create("car_can_thread", car_can_entry, RT_NULL, 2048, 13, 5);

    if (pc_uart_recive_thread != RT_NULL)
    {
        rt_thread_startup(pc_uart_recive_thread);
    }
    else
    {
        LOG_W("pc_uart_recive_thread ERR!");
    }
    if (pc_uart_send_thread != RT_NULL)
    {
        rt_thread_startup(pc_uart_send_thread);
    }
    else
    {
        LOG_W("pc_uart_send_thread ERR!");
    }
    if (ldv_uart_recive_thread != RT_NULL)
    {
        rt_thread_startup(ldv_uart_recive_thread);
    }
    else
    {
        LOG_W("ldv_uart_recive_thread ERR!");
    }
    if (car_can_thread != RT_NULL)
    {
        rt_thread_startup(car_can_thread);
    }
    else
    {
        LOG_W("car_can_thread ERR!");
    }
    if (flash_spi_thread != RT_NULL)
    {
        rt_thread_startup(flash_spi_thread);
    }
    else
    {
        LOG_W("flash_spi_thread ERR!");
    }
    rt_uint32_t ex_trigger_pin1 = rt_pin_get(EX_TRIGGER_PIN1);
    rt_err_t err = rt_pin_attach_irq(ex_trigger_pin1, PIN_IRQ_MODE_RISING, ex_irq14_callback, RT_NULL);
    err = rt_pin_irq_enable(ex_trigger_pin1, PIN_IRQ_ENABLE);
    if(RT_EOK != err)
    {
        LOG_W("EX_TRIGGER ERR!");
    }

}
/**********************************************************************/

void ldv_uart_recive_entry(void *parameter)
{

   u5_dev = rt_device_find("uart5");
   u6_dev = rt_device_find("uart6");
   u7_dev = rt_device_find("uart7");
   if (u5_dev != RT_NULL)
   {
       if(rt_device_open(u5_dev,RT_DEVICE_FLAG_RX_NON_BLOCKING | RT_DEVICE_FLAG_TX_NON_BLOCKING) == RT_EOK)
       {
           LOG_I("uart5 dev opened!");
       }
       else
       {
           LOG_W("uart5 dev err!");
       }
       rt_device_set_rx_indicate(u5_dev,uart_rx_ind);
   }
   if (u6_dev != RT_NULL)
   {
       if(rt_device_open(u6_dev,RT_DEVICE_FLAG_RX_NON_BLOCKING | RT_DEVICE_FLAG_TX_NON_BLOCKING) == RT_EOK)
       {
           LOG_I("uart6 dev opened!");
       }
       else
       {
           LOG_W("uart6 dev err!");
       }
       rt_device_set_rx_indicate(u6_dev,uart_rx_ind);
   }
   if (u7_dev != RT_NULL)
   {
       if(rt_device_open(u7_dev,RT_DEVICE_FLAG_RX_NON_BLOCKING | RT_DEVICE_FLAG_TX_NON_BLOCKING) == RT_EOK)
       {
           LOG_I("uart7 dev opened!");
       }
       else
       {
           LOG_W("uart7 dev err!\n");
       }
       rt_device_set_rx_indicate(u7_dev,uart_rx_ind);
   }

   rt_uint8_t u5_rcv_buf[SERIAL_RB_BUFSZ], u6_rcv_buf[SERIAL_RB_BUFSZ], u7_rcv_buf[SERIAL_RB_BUFSZ];//数据读取缓存
   rt_uint8_t u5_read_buf[SERIAL_RB_BUFSZ],u6_read_buf[SERIAL_RB_BUFSZ],u7_read_buf[SERIAL_RB_BUFSZ];//数据解析缓存
   struct user_queue_u8 u5_read_queue_ctrl,u6_read_queue_ctrl,u7_read_queue_ctrl;
   user_queue_u8_init(&u5_read_queue_ctrl,u5_read_buf,sizeof(u5_read_buf));
   user_queue_u8_init(&u6_read_queue_ctrl,u6_read_buf,sizeof(u6_read_buf));
   user_queue_u8_init(&u7_read_queue_ctrl,u7_read_buf,sizeof(u7_read_buf));
   rt_uint8_t UART5_PARSE_STATE = UART_PARSE_FIND_HEAD;
   rt_uint8_t UART6_PARSE_STATE = UART_PARSE_FIND_HEAD;
   rt_uint8_t UART7_PARSE_STATE = UART_PARSE_FIND_HEAD;
   rt_uint16_t rcv_len = 0;//串口设备实际读取到的数据量（字节）
   rt_uint16_t u5_read_len,u6_read_len,u7_read_len;//从接收缓存读到解析缓存队列的数据数量
   rt_uint8_t u5_rcv_frame_len,u6_rcv_frame_len,u7_rcv_frame_len;//当前接收帧的应有长度
   rt_uint8_t u5_rcv_frame_id,u6_rcv_frame_id,u7_rcv_frame_id;//当前接收数据帧ID

   struct frame_structure s_board_frame =//信号板帧解析参数
            {
                .head = {0XA5,0X5A},
                .head_len = 2,
                .device_addr = 0X80,
                .device_addr_mask = 0XC0,
                .frame_len_mask = 0X3F,
                .frame_data_buf = u5_read_buf
            };

   LOG_I("ldv_uart_recive_thread runing!");
   while(1)
   {
        if (rt_event_recv(uart_recive_ctrl,
                        (UART5_RCV_EVENT | UART6_RCV_EVENT | UART7_RCV_EVENT),
                        (RT_EVENT_FLAG_OR | RT_EVENT_FLAG_CLEAR),
                        RT_WAITING_FOREVER,
                        &ldv_rcv_event) == RT_EOK)
        {

            if((ldv_rcv_event & UART5_RCV_EVENT) == UART5_RCV_EVENT)
            {
                rcv_len = rt_device_read(u5_dev, 0, u5_rcv_buf, SERIAL_RB_BUFSZ);
                for (int i = 0; i < rcv_len; i++)
                {
                    user_queue_u8_putchar(&u5_read_queue_ctrl, u5_rcv_buf[i]);
                    switch (UART5_PARSE_STATE)
                    {
                        case UART_PARSE_FIND_HEAD:
                        {
                            do
                            {
                                u5_read_len = user_queue_u8_get_size(&u5_read_queue_ctrl);
                                if(u5_read_len > s_board_frame.head_len)
                                {
                                    if (s_board_frame.head[0] == user_queue_u8_readchar(&u5_read_queue_ctrl, 0) &&
                                        s_board_frame.head[1] == user_queue_u8_readchar(&u5_read_queue_ctrl, 1) &&
                                        s_board_frame.device_addr ==
                                        (user_queue_u8_readchar(&u5_read_queue_ctrl, 2)  & s_board_frame.device_addr_mask))

                                    {
                                        u5_rcv_frame_len = user_queue_u8_readchar(&u5_read_queue_ctrl, 2) & s_board_frame.frame_len_mask;
                                        UART5_PARSE_STATE = UART_PARSE_CHECK;
                                        break;
                                    }
                                    else
                                    {
                                        user_queue_u8_headmove(&u5_read_queue_ctrl, 1);
                                    }
                                }

                            }while(u5_read_len > s_board_frame.head_len);

                        }break;
                        case UART_PARSE_CHECK:
                        {
                            u5_read_len = user_queue_u8_get_size(&u5_read_queue_ctrl);
                            if(u5_read_len >= u5_rcv_frame_len)
                            {

                                static cnt_temp = 0;
                                if(cnt_temp++ >= 10)
                                {
                                    if (rt_pin_read(LED3_PIN))
                                    {
                                        rt_pin_write(LED3_PIN, PIN_LOW);
                                    }
                                    else
                                    {
                                        rt_pin_write(LED3_PIN, PIN_HIGH);
                                    }
                                    cnt_temp = 0;
                                }

                                rt_uint8_t sum = 0;
                                for (int i = 0; i < u5_rcv_frame_len - 3; i++)//和校验
                                {
                                    sum += user_queue_u8_readchar(&u5_read_queue_ctrl, 2+i);
                                }
                                if (sum == user_queue_u8_readchar(&u5_read_queue_ctrl,u5_rcv_frame_len -1))
                                {
                                    u5_rcv_frame_id = user_queue_u8_readchar(&u5_read_queue_ctrl,3);//帧ID检查
                                    switch (u5_rcv_frame_id)
                                    {
                                        case 0XD0://数据帧ID
                                        {
                                            if (u5_rcv_frame_len == 34 + 5)
                                            {
                                                rt_uint32_t tim_us = 0;
                                                rt_hwtimerval_t t = {};
                                                rt_device_read(hw_timer0, 0, &t, sizeof(t));
                                                tim_us = t.sec * 1000000 + t.usec;
                                                sBoardData.time_us = tim_us;
                                                user_queue_u8_headmove(&u5_read_queue_ctrl, 4);
                                                user_queue_u8_get(&u5_read_queue_ctrl,(rt_uint8_t*)&f_val, 4);
                                                sBoardData.velocityA = f_val;
                                                user_queue_u8_get(&u5_read_queue_ctrl,(rt_uint8_t*)&u16_val, 2);
                                                sBoardData.QsA = u16_val;
                                                user_queue_u8_get(&u5_read_queue_ctrl,(rt_uint8_t*)&f_val, 4);
                                                sBoardData.velocityB = f_val;
                                                user_queue_u8_get(&u5_read_queue_ctrl,(rt_uint8_t*)&u16_val, 2);
                                                sBoardData.QsB = u16_val;
                                                user_queue_u8_get(&u5_read_queue_ctrl,(rt_uint8_t*)&u16_val, 2);
                                                sBoardData.Qn = u16_val;
                                                user_queue_u8_get(&u5_read_queue_ctrl,(rt_uint8_t*)&f_val, 4);
                                                sBoardData.velocity = f_val;
                                                user_queue_u8_get(&u5_read_queue_ctrl,(rt_uint8_t*)&df_val, 8);
                                                sBoardData.distance = df_val;
                                                user_queue_u8_headmove(&u5_read_queue_ctrl, 6);
                                                sBoardData.sys_sta = user_queue_u8_getchar(&u5_read_queue_ctrl) & 0XF;
                                                sBoardData.frame_ID = user_queue_u8_getchar(&u5_read_queue_ctrl);
                                                user_queue_u8_headmove(&u5_read_queue_ctrl, 1);//移除和校验位

                                                rt_memcpy(ldv_to_pc_buf+4, &sBoardData.time_us, 4);
                                                rt_memcpy(ldv_to_pc_buf+8, &sBoardData.velocityA, 4);
                                                rt_memcpy(ldv_to_pc_buf+12, &sBoardData.QsA, 2);
                                                rt_memcpy(ldv_to_pc_buf+14, &sBoardData.velocityB, 4);
                                                rt_memcpy(ldv_to_pc_buf+18, &sBoardData.QsB, 2);
                                                rt_memcpy(ldv_to_pc_buf+20, &sBoardData.Qn, 2);
                                                rt_memcpy(ldv_to_pc_buf+22, &sBoardData.velocity, 4);
                                                rt_memcpy(ldv_to_pc_buf+26, &sBoardData.distance, 8);
                                                rt_memcpy(ldv_to_pc_buf+34, &sBoardData.sys_sta, 1);
                                                rt_memcpy(ldv_to_pc_buf+35, &sBoardData.frame_ID, 1);



                                                rt_mq_send(pc_send_msg, ldv_to_pc_buf, sizeof(ldv_to_pc_buf));
                                            }

                                        }break;

                                        default:
                                            user_queue_u8_headmove(&u5_read_queue_ctrl, u5_rcv_frame_len);//移除整帧
                                            break;
                                    }
                                }
                                else
                                {
                                    user_queue_u8_headmove(&u5_read_queue_ctrl, 1);//和校验失败，重新寻找帧头
                                }
                                UART5_PARSE_STATE = UART_PARSE_FIND_HEAD;
                            }

                        }break;
                        default:
                            break;
                    }
                }
            }
            else if ((ldv_rcv_event & UART6_RCV_EVENT) == UART6_RCV_EVENT)
            {
                rcv_len = rt_device_read(u6_dev, 0, u6_rcv_buf, SERIAL_RB_BUFSZ);
            }
            else if ((ldv_rcv_event & UART7_RCV_EVENT) == UART7_RCV_EVENT)
            {
                rcv_len = rt_device_read(u7_dev, 0, u7_rcv_buf, SERIAL_RB_BUFSZ);
            }

        }
   }

}
rt_err_t uart_rx_ind(rt_device_t dev, rt_size_t size)
{

   rt_uint32_t set_val;
   if(dev == u5_dev)
   {
       set_val = UART5_RCV_EVENT;
       rt_event_send(uart_recive_ctrl, set_val);
   }

   else if(dev == u6_dev)
   {
       set_val = UART6_RCV_EVENT;
       rt_event_send(uart_recive_ctrl, set_val);
   }
   else if(dev == u7_dev)
   {
       set_val = UART7_RCV_EVENT;
       rt_event_send(uart_recive_ctrl, set_val);

   }
   else if(dev == pc_uart)
   {
       set_val = UART1_RCV_EVENT;
       rt_event_send(pc_uart_event, set_val);
   }

   return RT_EOK;
}

extern CarCtrlParam car_param;
void pc_uart_entry(void *parameter)
{

    pc_uart = rt_device_find(PC_UART);
    if(pc_uart != RT_NULL)
    {
        if(rt_device_open(pc_uart,RT_DEVICE_FLAG_RX_NON_BLOCKING | RT_DEVICE_FLAG_TX_NON_BLOCKING) == RT_EOK)
        {
            LOG_I("pc_uart dev opened!");
        }
        else
        {
            LOG_W("pc_uart dev err!");
        }
        rt_device_set_rx_indicate(pc_uart,uart_rx_ind);
        rt_device_set_tx_complete(pc_uart, pc_uart_tx_ind);
    }
    else
    {
        LOG_W("no find pc_uart");
    }
    rt_hwtimer_mode_t hwtimer0_mode = HWTIMER_MODE_PERIOD;//Timer0定时模式
    rt_hwtimerval_t hwtimer0_out = {.sec = 0,.usec = 100000};//Timer0定时时间

    hw_timer0 = rt_device_find("hwtimer0");
    if (hw_timer0 != RT_NULL)
    {
        if(RT_EOK == rt_device_open(hw_timer0, RT_DEVICE_OFLAG_RDWR))
        {
            LOG_I("hwtimer0 dev opened!");
        }
        else
        {
            LOG_W("hwtimer0 dev open err!");
        }
        rt_device_set_rx_indicate(hw_timer0, hwtimer0_out_ind);
        rt_device_control(hw_timer0,HWTIMER_CTRL_MODE_SET,&hwtimer0_mode);
        rt_device_write(hw_timer0, 0, &hwtimer0_out, sizeof(hwtimer0_out));
    }

    rt_uint8_t pc_rcv_buf[SERIAL_RB_BUFSZ];//数据读取缓存
    rt_uint8_t pc_read_buf[SERIAL_RB_BUFSZ];//数据解析缓存
    struct user_queue_u8 pc_read_queue_ctrl;
    user_queue_u8_init(&pc_read_queue_ctrl,pc_read_buf,sizeof(pc_read_buf));
    rt_uint8_t UART1_PARSE_STATE = UART_PARSE_FIND_HEAD;
    rt_uint16_t pc_rcv_len = 0;//串口设备实际读取到的数据量（字节）
    rt_uint16_t pc_read_len;//从接收缓存读到解析缓存队列的数据数量
    rt_uint8_t pc_rcv_frame_len;//当前接收数据帧的应有长度
    rt_uint8_t pc_rcv_frame_id;//当前接收帧ID
    rt_uint8_t pc_rcv_frame_ft;//当前接收帧功能
    struct pc_frame_structure pc_data_frame =
    {
            .head = {0XA6,0X6A},
            .head_len = 2,
            .frame_data_buf = pc_rcv_buf
    };
    LOG_I("pc_recive_thread runing!");

    while(1)
    {

        if (rt_event_recv(pc_uart_event,
                UART1_RCV_EVENT,
                (RT_EVENT_FLAG_OR | RT_EVENT_FLAG_CLEAR),
                RT_WAITING_FOREVER,
                &pc_rcv_event) == RT_EOK)
        {
            if ((pc_rcv_event & UART1_RCV_EVENT) == UART1_RCV_EVENT)
            {
                pc_rcv_len = rt_device_read(pc_uart, 0, pc_rcv_buf, SERIAL_RB_BUFSZ);
                for (int i = 0; i < pc_rcv_len; i++)
                {
                    user_queue_u8_putchar(&pc_read_queue_ctrl, pc_rcv_buf[i]);
                    switch (UART1_PARSE_STATE)
                    {
                        case UART_PARSE_FIND_HEAD:
                        {
                            do
                            {
                                pc_read_len = user_queue_u8_get_size(&pc_read_queue_ctrl);
                                if (pc_read_len > pc_data_frame.head_len)
                                {
                                    if (user_queue_u8_readchar(&pc_read_queue_ctrl, 0) == pc_data_frame.head[0] &&
                                        user_queue_u8_readchar(&pc_read_queue_ctrl, 1) == pc_data_frame.head[1] )
                                    {
                                        pc_rcv_frame_len = user_queue_u8_readchar(&pc_read_queue_ctrl, 2);

                                        UART1_PARSE_STATE = UART_PARSE_CHECK;
                                        break;
                                    }
                                    else
                                    {
                                        user_queue_u8_headmove(&pc_read_queue_ctrl, 1);
                                    }
                                }

                            } while (pc_read_len > pc_data_frame.head_len);
                        }break;
                        case UART_PARSE_CHECK:
                        {
                            pc_read_len = user_queue_u8_get_size(&pc_read_queue_ctrl);
                            if (pc_read_len >= pc_rcv_frame_len)
                            {
                                rt_uint8_t sum = 0;
                                for (int i = 0; i < pc_rcv_frame_len - 3; i++)
                                {
                                    sum += user_queue_u8_readchar(&pc_read_queue_ctrl, 2+i);
                                }

                                static cnt_temp = 0;
                                if(cnt_temp++ >= 10)
                                {
                                    if (rt_pin_read(LED2_PIN))
                                    {
                                        rt_pin_write(LED2_PIN, PIN_LOW);
                                    }
                                    else
                                    {
                                        rt_pin_write(LED2_PIN, PIN_HIGH);
                                    }
                                    cnt_temp = 0;
                                }

                                if (sum == user_queue_u8_readchar(&pc_read_queue_ctrl,pc_rcv_frame_len -1))
                                {

                                    pc_rcv_frame_id = user_queue_u8_readchar(&pc_read_queue_ctrl, 3) & 0XF0;
                                    pc_rcv_frame_ft = user_queue_u8_readchar(&pc_read_queue_ctrl, 3) & 0X0F;

                                    switch (pc_rcv_frame_id)
                                    {
                                        case 0XC0:
                                        {
                                            if (pc_rcv_frame_ft == 0X00)//读PID 配置参数
                                            {
                                                ReadConfigFile();
                                                rt_memcpy(pid_to_pc_buf + 4, &saved_param.Ctrl_PID_AP, 4);
                                                rt_memcpy(pid_to_pc_buf + 8, &saved_param.Ctrl_PID_AI, 4);
                                                rt_memcpy(pid_to_pc_buf + 12, &saved_param.Ctrl_PID_AD, 4);
                                                rt_kprintf("read flash\n");
                                                rt_mq_send(pc_send_msg, pid_to_pc_buf, sizeof(pid_to_pc_buf));
                                                user_queue_u8_headmove(&pc_read_queue_ctrl, 4);
                                            }
                                            else if (pc_rcv_frame_ft == 0X01)//修改（保存）PID参数
                                            {
                                                rt_uint8_t pid_save_flag = 0;
                                                float val = 0;
                                                user_queue_u8_headmove(&pc_read_queue_ctrl, 4);
                                                user_queue_u8_get(&pc_read_queue_ctrl, (rt_uint8_t*)&val, 4);
                                                car_param.Ctrl_PID_AP = val;
                                                user_queue_u8_get(&pc_read_queue_ctrl, (rt_uint8_t*)&val, 4);
                                                car_param.Ctrl_PID_AI = val;
                                                user_queue_u8_get(&pc_read_queue_ctrl, (rt_uint8_t*)&val, 4);
                                                car_param.Ctrl_PID_AD = val;
                                                user_queue_u8_get(&pc_read_queue_ctrl, (rt_uint8_t*)&val, 4);
                                                car_param.Ctrl_carSpeed = val;
                                                pid_save_flag = user_queue_u8_getchar(&pc_read_queue_ctrl);
                                                car_pid_en = user_queue_u8_getchar(&pc_read_queue_ctrl);
                                                if(car_pid_en)
                                                    rt_kprintf("car start\n");
                                                if(pid_save_flag)
                                                {
                                                    saving_param.Ctrl_PID_AP =  car_param.Ctrl_PID_AP;
                                                    saving_param.Ctrl_PID_AI =  car_param.Ctrl_PID_AI;
                                                    saving_param.Ctrl_PID_AD =  car_param.Ctrl_PID_AD;
                                                    WriteConfigFile();
                                                    rt_kprintf("write flash\n");
                                                }

                                                user_queue_u8_headmove(&pc_read_queue_ctrl, 1);
                                            }
                                            else if (pc_rcv_frame_ft == 0X02)//误差
                                            {
                                                if (pc_rcv_frame_len == 5 + 5)
                                                {
                                                    static rt_uint8_t alive_cnt_check;
                                                    float dir_err = 0;
                                                    user_queue_u8_headmove(&pc_read_queue_ctrl, 4);
                                                    user_queue_u8_get(&pc_read_queue_ctrl, &dir_err, 4);
                                                    car_dir_err = dir_err;
                                                    if(alive_cnt_check != user_queue_u8_readchar(&pc_read_queue_ctrl, 0))
                                                    {
                                                        alive_cnt_check = user_queue_u8_readchar(&pc_read_queue_ctrl, 0);
                                                        pc_alive_cnt = 0;
                                                    }
                                                    user_queue_u8_headmove(&pc_read_queue_ctrl, 2);

                                                }
                                            }
                                        }break;

                                        default:
                                            user_queue_u8_headmove(&pc_read_queue_ctrl, pc_rcv_frame_len);
                                            break;
                                    }
                                }
                                else
                                {
                                    user_queue_u8_headmove(&pc_read_queue_ctrl, 1);//和校验失败，重新寻找帧头
                                }
                                UART1_PARSE_STATE = UART_PARSE_FIND_HEAD;
                            }

                        }break;
                        default:
                            break;
                    }
                }

            }

        }

    }
}
void pc_uart_send_entry(void *parameter)
{
    ReadConfigFile();
    rt_memcpy(pid_to_pc_buf + 4, &saved_param.Ctrl_PID_AP, 4);
    rt_memcpy(pid_to_pc_buf + 8, &saved_param.Ctrl_PID_AI, 4);
    rt_memcpy(pid_to_pc_buf + 12, &saved_param.Ctrl_PID_AD, 4);
    pid_to_pc_buf[16] = 0;
    for (int i = 0; i < 14; i++)
    {
        pid_to_pc_buf[16] += pid_to_pc_buf[i+2];
    }
    rt_device_write(pc_uart, 0, pid_to_pc_buf, sizeof(pid_to_pc_buf));


    pc_send_msg = rt_mq_create("pc_send_msg", 40, 30, RT_IPC_FLAG_FIFO);
    while(1)
    {
        if(RT_EOK == rt_mq_recv(pc_send_msg, msg_rcv_buf, sizeof(msg_rcv_buf), RT_WAITING_FOREVER))
        {
            rt_uint8_t len = msg_rcv_buf[2];
            msg_rcv_buf[len - 1] = 0;
            for (int i = 0; i < len - 3; i++)
            {
                msg_rcv_buf[len - 1] += msg_rcv_buf[2+i];
            }
           int sta = rt_device_write(pc_uart, 0, msg_rcv_buf, len);
           if (sta == 0)
           {
               static cnt = 0;
               if(cnt++ < 10)
               {
                   LOG_E("PC UART WRITE ERR");
               }
           }
        }
    }
}
rt_err_t  hwtimer0_out_ind(rt_device_t dev, rt_size_t size)
{
    return RT_EOK;
}
rt_err_t pc_uart_tx_ind(rt_device_t dev,  void *buffer)
{
    return RT_EOK;
}
void car_can_entry(void *parameter)
{
    Pid_ctrl_init();
    car_can = rt_device_find("can0");
    if (car_can != RT_NULL)
    {
        if(RT_EOK == rt_device_open(car_can, RT_DEVICE_FLAG_INT_TX | RT_DEVICE_FLAG_INT_RX))
        {
            LOG_I("can0 dev opened");
        }
        else
        {
            LOG_W("can0 dev err");
        }

    }
    else
    {
        LOG_E("no find can0 dev");
    }
    rt_device_control(car_can, RT_CAN_CMD_SET_BAUD, (void *)CAN500kBaud);
    rt_device_control(car_can, RT_CAN_CMD_SET_MODE, (void *)RT_CAN_MODE_NORMAL);

//    car_can_filter.id = 0x1;/* 报文 ID */
//    car_can_filter.ide = 0X01;/* 扩展帧标识位 */
//    car_can_filter.rtr = 0x00;/* 远程帧标识位 */
//    car_can_filter.mode = 0X01; /* 过滤表模式 */
//    car_can_filter.mask = 0X00;/* ID 掩码，0 表示对应的位不关心，1 表示对应的位必须匹配 */
//    car_can_filter.hdr = -1;/* -1 表示不指定过滤表号，对应的过滤表控制块也不会被初始化，正数为过滤表号，对应的过滤表控制块会被初始化 */
//    struct rt_can_filter_config car_can_cfg = {1,0,&car_can_filter};
//    rt_device_control(car_can, RT_CAN_CMD_SET_FILTER, &car_can_cfg);//配置接收过滤表

    car_rx_sem = rt_sem_create("car_rx_sem", 0, RT_IPC_FLAG_PRIO);//创建小车CAN数据接收信号量
    rt_device_set_rx_indicate(car_can, car_rx_callback);

    car_timer = rt_timer_create("car_timer", car_timer_out_ind, RT_NULL, car_timer_outcnt, RT_TIMER_FLAG_PERIODIC | RT_TIMER_FLAG_SOFT_TIMER);
    if (car_timer != RT_NULL)
    {
        rt_timer_start(car_timer);
    }
    LOG_I("car_can_thread runing!");

    while(1)
    {
        /* hdr 值为 - 1，表示直接从 uselist 链表读取数据 */
        can_rcv_buf.hdr = -1;

        /* 阻塞等待接收信号量 */
       if(RT_EOK == rt_sem_take(car_rx_sem, RT_WAITING_FOREVER))
       {
           /* 从 CAN 读取一帧数据 */
           rt_device_read(car_can, 0, &can_rcv_buf, sizeof(can_rcv_buf));
           if(can_rcv_buf.id == 0X18C4D2EF)//当前车速和转向角反馈
           {
               rt_uint32_t tim_us;
               rt_hwtimerval_t t;
               rt_uint8_t check = 0;

               check = can_rcv_buf.data[0] ^ can_rcv_buf.data[1] ^ can_rcv_buf.data[2] ^ can_rcv_buf.data[3] ^ can_rcv_buf.data[4] ^ can_rcv_buf.data[5] ^ can_rcv_buf.data[6];
               if(can_rcv_buf.data[7] == check)
               {

                   rt_uint64_t data_temp =0;
                   rt_uint16_t speed_temp;//当前车速 0.001m/s/bit
                   rt_int16_t angle_temp; //当前转向角 0.001°/s/bit
                   data_temp = *(rt_uint64_t*)(can_rcv_buf.data);
                   data_temp = data_temp >> 4;
                   speed_temp = (rt_uint16_t)data_temp;
                   angle_temp = (rt_int16_t)(data_temp >> 16);

//                   rt_kprintf("%d %d\n",speed_temp,angle_temp);
               }
           }
       }

    }
}
rt_err_t car_rx_callback(rt_device_t dev,rt_size_t size)
{
    rt_sem_release(car_rx_sem);
    return RT_EOK;
}
void car_timer_out_ind(void *parameter)
{
    static rt_uint16_t cnt = 0;
//    cnt++;//定时驻车计数 已关闭
    pc_alive_cnt++;//PC指令定时计数  超时未得到指令则驻车

    float angle_temp = 0;
    float speed_temp = 0;
    Pid_ctrl(car_pid_en, car_dir_err, &angle_temp,&speed_temp);

    car_send_parameter.angle = angle_temp*100;
    car_send_parameter.speed = speed_temp*1000;
    car_send_parameter.gear = car_pid_en ? 0x04 : 0x01;
    car_send_parameter.alive_cnt++;


    if (cnt == 200)
    {
        car_pid_en = 0;
        car_send_parameter.gear = 0x01;//10S后自动驻车
    }
    else if ((cnt > 200) && (car_pid_en == 1))
    {
        cnt = 0;
        car_send_parameter.gear = 0x04;
    }

    if(pc_alive_cnt >= 20)//PC指令无变化定时1S
    {
        car_pid_en = 0;
        car_send_parameter.gear = 0x01;//停车
    }

    rt_uint8_t send_buf[8];
    send_buf[0] = car_send_parameter.gear | (car_send_parameter.speed << 4);
    send_buf[1] = car_send_parameter.speed >> 4;
    send_buf[2] = (0x0F & (car_send_parameter.speed >> 12)) | (car_send_parameter.angle << 4);
    send_buf[3] = car_send_parameter.angle >> 4;
    send_buf[4] = 0x0F & (car_send_parameter.angle >> 12);
    send_buf[6] = car_send_parameter.alive_cnt << 4;
    send_buf[7] = (send_buf[0]^send_buf[1]^send_buf[2]^send_buf[3]^send_buf[4]^send_buf[5]^send_buf[6]);

    rt_memcpy(can_send_buf.data, send_buf, 8);
    rt_memcpy(car_to_pc_buf + 4,&car_send_parameter.angle, 2);
    rt_memcpy(car_to_pc_buf + 6,&car_send_parameter.speed, 2);
    rt_memcpy(car_to_pc_buf + 8,&car_dir_err, 4);

    if(RT_EOK != rt_mq_send(pc_send_msg, car_to_pc_buf, sizeof(car_to_pc_buf)))
    {
        LOG_W("IRQ MSG ERR");
    }
    rt_device_write(car_can, 0, &can_send_buf, sizeof(can_send_buf));
}

void ex_irq14_callback(void *args)
{
    rt_uint32_t tim_us;
    rt_hwtimerval_t t;

    ex_trigger_flag = !ex_trigger_flag;
    rt_pin_write(LED1_PIN, !ex_trigger_flag);

    rt_device_read(hw_timer0, 0, &t, sizeof(t));//读取当前时间
    tim_us = t.sec * 1000000 + t.usec;
    rt_memcpy(irq_to_pc_buf+4, &tim_us, 4);
    irq_to_pc_buf[8] = 0X01;//外部触发  有效
    rt_mq_send(pc_send_msg, irq_to_pc_buf, sizeof(irq_to_pc_buf));
}
void flash_spi_entry(void *parameter)
{

    while(1)
    {
        rt_thread_mdelay(500);

    }

}

static int rt_hw_spi_flash_init()//W25Q64 自动初始化
{
    rt_spi_bus_attach_device(&spi0_w25q64_dev, "spi01", "spi0", (void *)SPI0_CS);//spi01设备（W25Q64BV）挂载到SPI0总线上
    if (RT_NULL == rt_sfud_flash_probe("W25Q64BV", "spi01"))
    {
        return -RT_ERROR;
    }
}
INIT_COMPONENT_EXPORT(rt_hw_spi_flash_init);

void pid_en_change_cmd(void)
{
    car_pid_en = !car_pid_en;
}MSH_CMD_EXPORT(pid_en_change_cmd,pid_switch)

int spi_flash_mount(void)
{
    int sta = dfs_mount("W25Q64BV", "/W25Q64BV", "elm", 0, 0);
    return sta;
}

int flash_mkfs(void)
{
    int sta = dfs_mkfs("elm", "W25Q64BV");//文件系统格式化
    return sta;
}
static rt_err_t ReadConfigFile(void)
{
    config_fd = open(CONFIG_FILE_NAME,O_RDONLY | O_CREAT);
    if(config_fd > 0)
    {
        read(config_fd, &saved_param, sizeof(saved_param));
        close(config_fd);
    }
    else
    {
        LOG_W("cfg  file open err!");
    }
}
static rt_err_t WriteConfigFile(void)
{
    config_fd = open(CONFIG_FILE_NAME,O_CREAT | O_RDWR);
    if(config_fd > 0)
    {
        write(config_fd, &saving_param, sizeof(saving_param));
        close(config_fd);
    }
}

void flwrite()
{
    char test_str[512];
    rt_memset(test_str, 0X11, 512);
    int fd = open("/W25Q64BV/text.txt", O_WRONLY | O_CREAT | O_APPEND);
    if(fd >= 0)
    {
        write(fd, test_str, sizeof(test_str));
        close(fd);
        rt_kprintf("Write done.\n");
    }
    else
    {
        LOG_W("flash open err");
    }

}MSH_CMD_EXPORT(flwrite,w25q64 write)
void flread()
{
    char test_str[512];
    rt_memset(test_str, 0, 512);
    int fd = open("/W25Q64BV/text.txt", O_RDONLY);
    if(fd >= 0)
    {
        read(fd, test_str, sizeof(test_str));
        close(fd);
        rt_kprintf("Read done.\n");
        rt_device_write(pc_uart, 0, test_str, sizeof(test_str));
    }
    else
    {
        LOG_W("flash open err");
    }

}MSH_CMD_EXPORT(flread,w25q64 read)
